import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
from rclpy.qos import QoSProfile, ReliabilityPolicy
import tf2_ros

class OdomTopic2TF(Node):
    def __init__(self):
        super().__init__('odom2tf')
        # 设置为不可靠（BEST_EFFORT）的QoS策略
        qos_profile = QoSProfile(
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT  # 设置为不可靠性
        )
        # 创建 odom 话题订阅者，使用传感器数据的 Qos
        self.odom_subscribe = self.create_subscription(
            Odometry,
            'odom',
            self.odom_callback,
            10 # QoS参数，可以调整缓冲区大小
        )
        # 创建 TransformBroadcaster 用于广播坐标变换
        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)

    def odom_callback(self, msg):
        transform = TransformStamped()
        transform.header = msg.header  # 使用消息的时间戳和框架ID
        # transform._header.frame_id = "odom"
        # transform._header.stamp = msg.header.stamp
        # transform.child_frame_id = "base_footprint"
        transform.child_frame_id = msg.child_frame_id
        transform.transform.translation.x = msg.pose.pose.position.x
        transform.transform.translation.y = msg.pose.pose.position.y
        transform.transform.translation.z = msg.pose.pose.position.z
        transform.transform.rotation.x = msg.pose.pose.orientation.x
        transform.transform.rotation.y = msg.pose.pose.orientation.y
        transform.transform.rotation.z = msg.pose.pose.orientation.z
        transform.transform.rotation.w = msg.pose.pose.orientation.w
        # 广播坐标变换信息
        self.tf_broadcaster.sendTransform(transform)

def main(args=None):
    rclpy.init(args=args)
    odom2tf = OdomTopic2TF()
    rclpy.spin(odom2tf)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
